#include <rclcpp/rclcpp.hpp>

// MoveIt Servo 依赖
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2_ros/buffer.h>

#include <string>
#include <chrono>
#include <thread>
#include <mutex>
#include <memory>

// 终端键盘（POSIX）
#include <termios.h>
#include <unistd.h>

using namespace std::chrono_literals;

namespace velocity_node {

static rclcpp::Node::SharedPtr node_;
static rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("velocity_publisher");

// ======================= 新增：离散“脉冲”控制状态 =======================
struct PulseCmd {
  double lx{0}, ly{0}, lz{0};
  double ax{0}, ay{0}, az{0};
  std::chrono::steady_clock::time_point until{std::chrono::steady_clock::now()};
  bool active{false};
};

static std::mutex g_cmd_mtx;
static PulseCmd g_pulse;

// 基本参数（可调）
constexpr const char* FRAME_ID = "base_link";  // 保持与 Servo 配置一致
constexpr double LIN_SPEED = 0.02;             // m/s：每次脉冲的线速度大小
constexpr double ANG_SPEED = 0.50;             // rad/s：每次脉冲的角速度大小
constexpr double PULSE_TIME = 0.15;            // s：每次“动一下”的持续时间

// ======================= 新增：终端原始模式 RAII =======================
class TermiosGuard {
 public:
  TermiosGuard() : ok_(false) {
    if (tcgetattr(STDIN_FILENO, &old_) == 0) {
      termios raw = old_;
      raw.c_lflag &= ~(ICANON | ECHO);  // 关闭规范模式与回显
      raw.c_cc[VMIN] = 0;               // 非阻塞读取
      raw.c_cc[VTIME] = 0;
      if (tcsetattr(STDIN_FILENO, TCSANOW, &raw) == 0) ok_ = true;
    }
  }
  ~TermiosGuard() {
    if (ok_) tcsetattr(STDIN_FILENO, TCSANOW, &old_);
  }
 private:
  termios old_{};
  bool ok_;
};

// ======================= 新增：键盘监听线程 =======================
// 按键即触发一次“速度脉冲”，持续 PULSE_TIME 秒
static void keyboardThread() {
  TermiosGuard guard;

  RCLCPP_INFO(LOGGER,
              "\n[KEYS] 離散鍵盤控制已啟動：\n"
              "  平移:  w/s=+X/-X,  a/d=-Y/+Y,  r/f=+Z/-Z\n"
              "  旋轉:  i/k=+roll/-roll,  j/l=+yaw/-yaw,  u/o=+pitch/-pitch\n"
              "  空格:  立即停止   |   q: 退出節點\n");

  while (rclcpp::ok()) {
    unsigned char c = 0;
    ssize_t n = ::read(STDIN_FILENO, &c, 1);  // 非阻塞
    if (n == 1) {
      PulseCmd local{};
      bool make_pulse = true;

      switch (c) {
        // 平移
        case 'w': local.lx = +LIN_SPEED; break;
        case 's': local.lx = -LIN_SPEED; break;
        case 'a': local.ly = -LIN_SPEED; break;
        case 'd': local.ly = +LIN_SPEED; break;
        case 'r': local.lz = +LIN_SPEED; break;
        case 'f': local.lz = -LIN_SPEED; break;

        // 角速度（roll/pitch/yaw）
        case 'i': local.ax = +ANG_SPEED; break;   // +roll
        case 'k': local.ax = -ANG_SPEED; break;   // -roll
        case 'u': local.ay = +ANG_SPEED; break;   // +pitch
        case 'o': local.ay = -ANG_SPEED; break;   // -pitch
        case 'j': local.az = +ANG_SPEED; break;   // +yaw
        case 'l': local.az = -ANG_SPEED; break;   // -yaw

        // 空格：停止
        case ' ':
          make_pulse = false;
          {
            std::scoped_lock<std::mutex> lk(g_cmd_mtx);
            g_pulse = PulseCmd{};
            g_pulse.active = false;
          }
          RCLCPP_INFO(LOGGER, "[KEYS] STOP (space)");
          break;

        // q：退出
        case 'q':
          RCLCPP_INFO(LOGGER, "[KEYS] Quit requested (q).");
          rclcpp::shutdown();
          return;

        default:
          make_pulse = false;
          break;
      }

      if (make_pulse) {
        // 关键修复：把 duration<double> 转成 steady_clock::duration 再相加
        local.until =
            std::chrono::steady_clock::now() +
            std::chrono::duration_cast<std::chrono::steady_clock::duration>(
                std::chrono::duration<double>(PULSE_TIME));
        local.active = true;
        {
          std::scoped_lock<std::mutex> lk(g_cmd_mtx);
          g_pulse = local;
        }
        RCLCPP_INFO(LOGGER,
                    "[KEYS] Pulse lin(%.2f, %.2f, %.2f) m/s  ang(%.2f, %.2f, %.2f) rad/s  for %.0f ms",
                    local.lx, local.ly, local.lz, local.ax, local.ay, local.az,
                    PULSE_TIME * 1000.0);
      }
    }

    std::this_thread::sleep_for(5ms);
  }
}

// ======================= 修改：发布函数 - 根据脉冲状态发消息 =======================
// 控制循环：定时发布当前脉冲（若超时则发布 0）
static void publishTwistCommand() {
  PulseCmd cur;
  {
    std::scoped_lock<std::mutex> lk(g_cmd_mtx);
    if (g_pulse.active &&
        std::chrono::steady_clock::now() < g_pulse.until) {
      cur = g_pulse;
    } else {
      g_pulse.active = false;
      cur = PulseCmd{};
    }
  }

  auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
  msg->header.stamp = node_->now();
  msg->header.frame_id = FRAME_ID;

  msg->twist.linear.x  = cur.lx;
  msg->twist.linear.y  = cur.ly;
  msg->twist.linear.z  = cur.lz;
  msg->twist.angular.x = cur.ax;
  msg->twist.angular.y = cur.ay;
  msg->twist.angular.z = cur.az;

  twist_pub_->publish(std::move(msg));

  if (cur.lx || cur.ly || cur.lz || cur.ax || cur.ay || cur.az) {
    RCLCPP_INFO_THROTTLE(
      LOGGER, *node_->get_clock(), 500,
      "[VEL PUB] Pulse -> lin(%.3f, %.3f, %.3f) m/s | ang(%.3f, %.3f, %.3f) rad/s in frame: %s",
      cur.lx, cur.ly, cur.lz, cur.ax, cur.ay, cur.az, FRAME_ID);
  }
}

} // namespace velocity_node

int main(int argc, char** argv)
{
  using namespace velocity_node;

  rclcpp::init(argc, argv);
  rclcpp::NodeOptions opts;
  opts.use_intra_process_comms(false);
  node_ = std::make_shared<rclcpp::Node>("velocity_publisher", opts);

  // --- 1. MoveIt Planning Scene Monitor (Servo 依赖) ---
  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
  auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
      node_, "robot_description", tf_buffer, "planning_scene_monitor");
  if (psm->getPlanningScene()) {
    psm->startStateMonitor("/joint_states");
    psm->setPlanningScenePublishingFrequency(25);
    psm->startPublishingPlanningScene(
        planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
        "/moveit_servo/publish_planning_scene");
    psm->startSceneMonitor();
    psm->providePlanningSceneService();
  } else {
    RCLCPP_ERROR(LOGGER, "Planning scene 未正确配置");
    return EXIT_FAILURE;
  }

  // --- 2. MoveIt Servo (必须创建并 start) ---
  auto servo_params = moveit_servo::ServoParameters::makeServoParameters(node_);
  auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_params, psm);
  servo->start();

  // --- 3. 速度发布器 ---
  // 注意：Servo 默认监听 ~/delta_twist_cmds
  twist_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>(
      "~/delta_twist_cmds", 10);

  // --- 4. 启动键盘监听线程（非阻塞） ---
  std::thread(velocity_node::keyboardThread).detach();

  // --- 5. 控制循环：每 10ms 发布一次 ---
  auto timer = node_->create_wall_timer(10ms, &velocity_node::publishTwistCommand);

  // 使用多线程执行器
  auto exec = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
  exec->add_node(node_);
  exec->spin();

  rclcpp::shutdown();
  return 0;
}
